/*
 * Brain.cpp
 *
 *  Created on: Dec 20, 2012
 *      Author: mark
 */

#include "brain.h"

void	*DriveThread(void *arg);

static const	int	SLEEP_TIME	=	100000;		// equal to the uptime of a drive pulse

struct DriveThreadArgs {
	Ping				*ping;
	MotorController		*motorcontroller;
	Encoders			*encoders;
};

double Angle(double a, double o)
{
	double radians = atan(a / o);
	return radians * (180 / M_PI);
}

Brain::Brain()
{
	// Set the connection variables for the VN100
	const char	*port		=	"//dev//ttyUSB0";
	const int	baudRate	=	230400;

	// Connect to the VN100
	vn100_connect(&vn100, port, baudRate);

	// Connect to the encoders
	encoders = new Encoders();

	// Connect to the ping
	ping = new Ping();
}

Brain::~Brain()
{
	pthread_join(driveThread, NULL);
}

void Brain::Drive()
{
	// Create the thread arguments
	DriveThreadArgs *args = new DriveThreadArgs();
	args->ping				=	ping;
	args->motorcontroller	=	motorcontroller;
	args->encoders			=	encoders;

	// Start the thread
	pthread_create(&driveThread, NULL, DriveThread, (void *)args);
}

void *DriveThread(void *arg)
{
	DriveThreadArgs *args				=	(DriveThreadArgs *)arg;
	Ping			*ping				=	args->ping;
	MotorController	*motorcontroller	=	args->motorcontroller;
	Encoders		*encoders			=	args->encoders;

	int				distance			=	0;

	while(true) {
		// Drive as long as the wall is near
		while(ping->ObjectIsNear()) {
			while(distance < 100) {
				//motorcontroller->MoveForward();
				distance = 2.042 * encoders->GetLeftRotations();

				cout << distance << endl;

				usleep(SLEEP_TIME);
			}
			return (void *)NULL;
		}

		// Turn until the wall is near
		while(!ping->ObjectIsNear()) {
		}
	}

	return (void *)NULL;
}

void Brain::Navigate()
{
}

double Brain::CalculateDesiredYaw()
{
	double		distance_x	=	0,		distance_y	=	0;
	double		angle;
	VnYpr		ypr;

	// X distance
	if (position->x		<	destination->x) {
		distance_x = destination->x - position->x;
	}
	else {
		distance_x = position->x -destination->x;
	}

	// Y distance
	if (position->y		<	destination->y) {
		distance_y = destination->y - position->y;
	}
	else {
		distance_y = position->y - position->y;
	}

	// Angle
	angle = Angle(distance_x, distance_y);

	// Get the yaw
	vn100_getYawPitchRoll(&vn100, &ypr);
	yaw = ypr.yaw;

	// FALSE RETURN VALUE
	return angle;
}





































